#include <cv.h>
#include <highgui.h>

#include "faceDetector.h"

using namespace cv;

//TODO:
//CONSIDER
//Morphological ops							impl, untest
//Backsub
//TorsoROI									done, tested somewhat
//Shrunken faceROI?							done, tested somewhat, works well
		//face region tends to be too wide
//Patchbased
//thresholding histos
//Mean-shift seg to update histo
//Edge preserving smoothing
//Adaptive thresholding

//meanshift + basic init guess				done, tested somewhat, has problems
//meanshift + init guess

//camshift, init guess : almost all frame	done, tested somewhat, end frame a bit large?

//FIX: Deallocation!

int main( int argc, char** argv )
{
    Mat src;
    if( argc != 2 || !(src=imread(argv[1], 1)).data )
        return -1;
	
    Mat hsvFullSize;
    cvtColor(src, hsvFullSize, CV_BGR2HSV);
	
	FaceDetector* fd = new FaceDetector(1.0);
	
	vector<Rect>* faces = fd->detectFace(src);
	
	if (!faces->empty()) {
		
		Rect faceRect = faces->front();
		
		Rect narrowFaceRect = Rect( faceRect.x+faceRect.width/6, faceRect.y, 2*faceRect.width/3, faceRect.height);
		
		//Draw full face rect
		rectangle( src, Point( faceRect.x, faceRect.y ), //WAS bug : cvRectangle instead
				  Point( faceRect.x+faceRect.width, faceRect.y + faceRect.height ),
				  Scalar::all(0),
				  2);		
		//Draw narrow face rect
		rectangle( src, Point( narrowFaceRect.x, narrowFaceRect.y ), //WAS bug : cvRectangle instead
				  Point( narrowFaceRect.x+narrowFaceRect.width, narrowFaceRect.y + narrowFaceRect.height ),
				  Scalar::all(100),
				  2);
	
		Mat hsvFace( hsvFullSize, narrowFaceRect );
		
		// let's quantize the hue to 30 levels
		// and the saturation to 32 levels
		int hbins = 30, sbins = 32;
		int histSize[] = {hbins, sbins};
		// hue varies from 0 to 179, see cvtColor
		float hranges[] = { 0, 180 };
		// saturation varies from 0 (black-gray-white) to
		// 255 (pure spectrum color)
		float sranges[] = { 0, 256 };
		const float* ranges[] = { hranges, sranges };
		MatND hist;
		// we compute the histogram from the 0-th and 1-st channels
		int channels[] = {0, 1};
		//int *channels2 = &channels[0];
		
		calcHist( &hsvFace, 1, channels, Mat(), // do not use mask
				 hist, 2, histSize, ranges,
				 true, // the histogram is uniform
				 false );
		double maxVal = 0;
		minMaxLoc(hist, 0, &maxVal, 0, 0);
		
		int scale = 10;
		Mat histImg = Mat::zeros(sbins*scale, hbins*10, CV_8UC3);
		
		for( int h = 0; h < hbins; h++ )
			for( int s = 0; s < sbins; s++ )
			{
				float binVal = hist.at<float>(h, s);
				int intensity = cvRound(binVal*255/maxVal); //WAS bug : maxValue instead of maxVal
				rectangle( histImg, Point(h*scale, s*scale), //WAS bug : cvRectangle instead
							Point( (h+1)*scale - 1, (s+1)*scale - 1),
							Scalar::all(intensity),
							CV_FILLED );
			}
		
		//Back proj
		
		//Torso start set to 1 face width to the left of face start,
		//or if that's outside the frame, just 0.
		//NOTE: Not using narrowFaceRect sizes!
		//TODO: Determine good torso size
		int torsoApproxStartX = ( faceRect.x > faceRect.width ) ? faceRect.x - faceRect.width : 0;
		
		
		//Torso width set to 3 face width, or maximum possible,
		//if that is too much to fit inside frame.
		int torsoApproxWidth = ( src.cols > 3*faceRect.width ) ? 3*faceRect.width : src.cols - torsoApproxStartX;
		
		//FIX : throw exception if face is too near bottom?
		int neckTop = (src.rows > faceRect.y + 3*faceRect.width/2) ? faceRect.y + 3*faceRect.width/2 : faceRect.y;
		
		Rect torsoApproxRect( torsoApproxStartX, neckTop,
							torsoApproxWidth, src.rows - neckTop);
		
		
		rectangle( src, Point( torsoApproxStartX, neckTop ), //WAS bug : cvRectangle instead
				  Point( torsoApproxStartX + torsoApproxWidth, src.rows ),
				  Scalar::all(0),
				  2);
		
		Mat hsvTorso( hsvFullSize, torsoApproxRect);
		
		
		
		Mat backProj = Mat::zeros(hsvTorso.cols, hsvTorso.rows, CV_8UC1);
		
		std::cout << "to here 2" << std::endl;
		
		calcBackProject( &hsvTorso, 1, channels, hist, backProj, ranges, 1.0, true);
		
		namedWindow( "BackProj", 1 );
		imshow( "BackProj", backProj );
		
		//waitKey();

		//FIX : check in-place support
		morphologyEx( backProj, backProj, MORPH_OPEN, Mat(), Point(-1,-1), 1);
		//OPEN seems good
		
		//How many iter?
		
		//7 a bit many?
		
		//3 too many?
		
		//1 appears best,
		//at least on handL_back_dave.jpg,
		//while still removing most noise on handL_wbg6_fSTD_long.jpg
		
		//TODO : It appears to be worthwhile, but do test.
		

		
		
		//Finding the hand location!
		
		//NOTE, empirically, it seems a good bounding box for the hand is the full size face rect
		//BUT we need something much bigger?! We don't know where it starts off
		//OR : we could compute the starting location as a weighted average
		//OR : we could just use this starting location, if it's good. Probably not...
		//FIX : find good starting point!
		//FIX : ensure edge stability!!!
		//FIX : should use meanshift, camshift or just starting point?
		
		//FIX : bad sizes!!!
//		int handWidth = ( backProj.cols < faceRect.width ) ? backProj.cols : faceRect.width;
//		
//		int handHeight = ( backProj.rows < faceRect.height ) ? backProj.rows : faceRect.height;
//		
//		
//		Rect handRect = Rect( backProj.cols/2 - handWidth/2 , backProj.rows/2 - handHeight/2, handWidth , handHeight );
		
		const int initialHandRectBorder = 0;
		
		Rect handRect = Rect( initialHandRectBorder, initialHandRectBorder,
							 torsoApproxRect.width - initialHandRectBorder, torsoApproxRect.height - initialHandRectBorder );
		
		//FIX : find appropriate numbers!
		CamShift( backProj, handRect, TermCriteria( TermCriteria::MAX_ITER + TermCriteria::EPS , 60, 0.001) );
		
		rectangle( backProj, Point( handRect.x, handRect.y ), //WAS bug : cvRectangle instead
				  Point( handRect.x + handRect.width, handRect.y+handRect.height),
				  Scalar::all(250),
				  2);
		
		//FIX : reject if not hand shaped !
		//TODO : more advanced rejection techniques?
		
		
		
		namedWindow( "Source", 1 );
		imshow( "Source", src );
		
		namedWindow( "H-S Histogram", 1 );
		imshow( "H-S Histogram", histImg );
		
		namedWindow( "BackProjMorphed", 1 );
		imshow( "BackProjMorphed", backProj );
		
		//NOTE:
		
		waitKey();
		
		
		
	} else {
		
		std::cout << "no face found" << std::endl;
		
	}
}
